//
// Created by chen on 25-5-23.
//

#pragma once

#include "arm_math.h"
#include <cmath>
#include <cstddef>

// 前置声明硬件相关类型（若需要）
// class TIM_HandleTypeDef;
// 控制模式枚举
enum class ControlMode {
     kNull,          // 无控制
     kPosition,     // 位置控制
     kSpeed,        // 速度控制
     kTorque,       // 力矩控制
     kSpeedTorque,  // 速度-力矩控制
     kPositionSpeedTorque // 位置-速度-力矩控制
 };

// 控制参数结构体
struct ControlParams {
    float position;        // 目标角度 (rad)
    float speed;           // 目标速度 (rad/s)
    float torque_d;        // d轴力矩归一化值 (0~1)
    float torque_q;        // q轴力矩归一化值 (0~1)
    float max_speed;       // 最大速度 (rad/s)
    float max_torque;      // 最大力矩归一化值 (0~1)
    ControlMode mode;      // 当前控制模式
};

class CFOCController {
private:


    // PID控制器实例
    mutable arm_pid_instance_f32 m_pidPosition;
    mutable arm_pid_instance_f32 m_pidSpeed;
    mutable arm_pid_instance_f32 m_pidTorqueD;
    mutable arm_pid_instance_f32 m_pidTorqueQ;

    // 控制参数
    ControlParams m_controlParams;

    // 常量定义
    static constexpr float kPI = 3.14159265358979323f;
    static constexpr float kRad60 = kPI / 3.0f;
    static constexpr float kSqrt3 = 1.7320508075688772f;

    // 角度循环差分计算（声明为私有方法）
    float cycleDiff(float current, float target, float cycle) const;



    // 位置环控制（声明为私有方法）
    float runPositionLoop(float targetRad) const;



    // d轴电流环控制（声明为私有方法）
    float runTorqueDLoop(float targetNorm) const;

    // q轴电流环控制（声明为私有方法）
    float runTorqueQLoop(float targetNorm) const;

    // 积分饱和处理（声明为私有方法）
    void handleIntegralWindup(arm_pid_instance_f32& pid, float error, float output) const;

    // FOC前向计算（纯虚函数，子类必须实现）
    void applyPwm(float du, float dv, float dw) ;

public:
    // 构造函数
    CFOCController();
    // 速度环控制（声明为私有方法）
    float runSpeedLoop(float targetRadPerSec) const;

    // 重置PID参数为默认值
    void resetPidParameters();

    // SVPWM调制核心算法（声明为私有方法）
    void svpwm(float phi, float d, float q, float *d_u, float *d_v, float *d_w) ;

    // 设置PID参数
    void setPidGains(
        float posKp, float posKi, float posKd,
        float spdKp, float spdKi, float spdKd,
        float tdKp, float tdKi, float tdKd,
        float tqKp, float tqKi, float tqKd);

    // 控制模式接口

    // 位置控制
    void setPositionControl(float targetRad);

    // 速度控制
    void setSpeedControl(float targetRadPerSec);

    // 力矩控制（d/q轴独立）
    void setTorqueControl(float dNorm, float qNorm);

    // 速度-力矩串级控制
    void setSpeedTorqueControl(float speedRadPerSec, float maxTorqueNorm);

    // 位置-速度-力矩串级控制
    void setPositionSpeedTorqueControl(float posRad, float maxSpeed, float maxTorque);

    // 执行控制周期
    void update(float e_angle);

    // 角度单位转换（静态方法）
    static float degToRad(float deg);
    static float radToDeg(float rad);
};